// 激光雷达发布节点
#include "rclcpp/rclcpp.hpp"
#include "robot_control/msg/lidar_data.hpp"
#include "std_msgs/msg/header.hpp"

#include <chrono>
#include <random>

using namespace std::chrono_literals;

class LidarPublisher : public rclcpp::Node
{
public:
    LidarPublisher() : Node("lidar_publisher"), count_(0)
    {
        // 声明参数
        this->declare_parameter("publish_frequency", 10.0);
        // 获取参数
        double publish_frequency = this->get_parameter("publish_frequency").as_double();
        // 创建发布者
        publisher_ = this->create_publisher<robot_control::msg::LidarData>("/lidar_data", 10);
        // 创建定时器
        auto timer_period = std::chrono::milliseconds(static_cast<int>(1000.0 / publish_frequency));
        timer_ = this->create_wall_timer(
            timer_period,
            std::bind(&LidarPublisher::timer_callback, this));

        RCLCPP_INFO(this->get_logger(), "激光雷达发布节点已启动，频率:%.1f Hz", publish_frequency);
    }

private:
    void timer_callback()
    {
        auto message = robot_control::msg::LidarData();

        // 生成模拟激光雷达数据（十个方向的测距）
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_real_distribution<float> dis(0.1f, 5.0f); // 0.1-5.0米的随机距离

        for (int i = 0; i < 10; i++)
        {
            message.distances[i] = dis(gen);
        }

        // 设置时间戳
        message.stamp = this->now();

        // 发布消息
        publisher_->publish(message);

        count_++;
        if (count_ % 10 == 0)
        { // 每10次发布打印一次日志
            RCLCPP_DEBUG(this->get_logger(), "第 %ld 次发布激光雷达数据", count_);
        }
    }

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<robot_control::msg::LidarData>::SharedPtr publisher_;
    size_t count_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<LidarPublisher>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}